#include<fstream>
#include <iostream>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include<opencv2/objdetect/objdetect.hpp>
#include<opencv2/ml/ml.hpp>

using namespace cv;
using namespace std;

#define PosSamNo 2400          //positive samples
#define NegSamNo 12000         //negtive samples

#define TRAIN true       //whether training or not ,true means re-trainging,false means to SVM
#define CENTRAL_CROP true //true:96*160 crops to 64*128 in positive

#define HardExampleNo 3168 //hard examples



class MySVM : public CvSVM
{
public:
    //获得SVM的决策函数中的alpha数组
    double * get_alpha_vector()
    {
        return this->decision_func->alpha;
    }
    
    //获得SVM的决策函数中的rho参数,即偏移量
    float get_rho()
    {
        return this->decision_func->rho;
    }
};



int main()
{
    //detector window(64,128) block(16,16) stride(8,8) cell(8,8)bin:9
    HOGDescriptor hog(Size(64,128),Size(16,16),Size(8,8),Size(8,8),9);
    int DescriptorDim;//dimension of HOG
    MySVM svm;//classifier of SVM
    //-------------------------------------------------------------------------------------------
    //            TRAIN:true means re-training
    //-------------------------------------------------------------------------------------------
    if(TRAIN)
    {
        string ImgName;
        ifstream finPos("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/pos/INRIAPerson96x160PosList.txt");
        ifstream finNeg("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/neg/NoPersonFromINRIAList.txt");
        Mat sampleFeatureMat;//feature vector,rows:No of all samples,cols:dimension of HOG
        Mat sampleLabelMat;//label,rows:No of all samples,one col:1:person,-1:nonperson
        //-------------------------------------------------------------------------------------------
        //             reading positive image
        //-------------------------------------------------------------------------------------------
        for(int num=0;num!=PosSamNo && getline(finPos,ImgName);++num)
        {
            cout<<"processing: "<<ImgName<<endl;
            ImgName="/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/pos/"+ImgName;
            Mat src=imread(ImgName);
            if(CENTRAL_CROP)
                src=src(Rect(16,16,64,128));
            
            vector<float>descriptors;
            hog.compute(src,descriptors,Size(8,8));//compute HOG ,stride of Window(8,8)
            cout<<"dimension："<<descriptors.size()<<endl;
            
            //initialize
            if(0==num)
            {
                DescriptorDim = static_cast<int>(descriptors.size());//dimension of HOG
                sampleFeatureMat=Mat::zeros(PosSamNo+NegSamNo+HardExampleNo,DescriptorDim,CV_32FC1);
                sampleLabelMat=Mat::zeros(PosSamNo+NegSamNo+HardExampleNo,1,CV_32FC1);
            }
            //copy computed HOG to sampleFeatureMat
            for(int i=0;i!=DescriptorDim ;++i)
                sampleFeatureMat.at<float>(num,i)=descriptors[i];
            sampleLabelMat.at<float>(num,0)=1;
        }
        //------------------------------------------------------------------------------------------
        //              reading negtive image
        //-------------------------------------------------------------------------------------------
        for(int num=0;num!=NegSamNo && getline(finNeg,ImgName);++num)
        {
            
            cout<<"processing: "<<ImgName<<endl;
            ImgName="/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/neg/"+ImgName;
            Mat src=imread(ImgName);
            vector<float>descriptors;
            hog.compute(src,descriptors,Size(8,8));
            
            for(int i=0;i!=DescriptorDim;++i)
                sampleFeatureMat.at<float>(num+PosSamNo,i)=descriptors[i];
            sampleLabelMat.at<float>(num+PosSamNo,0)=-1;
        }
        //-------------------------------------------------------------------------------------------
        //              reading HardExample
        //-------------------------------------------------------------------------------------------
        if(HardExampleNo>0)
        {
            ifstream finHardExample("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/har_mix/HardExample_2400PosINRIA_12000NegList.txt");
            for(int num=0;num!=HardExampleNo && getline(finHardExample,ImgName);num++)
            {
                cout<<"processing: "<<ImgName<<endl;
                ImgName="/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/har_mix/"+ImgName;
                Mat src=imread(ImgName);
                vector<float>descriptors;
                hog.compute(src,descriptors,Size(8,8));
                
                
                for(int i=0;i!=DescriptorDim;++i)
                    sampleFeatureMat.at<float>(num+PosSamNo+NegSamNo,i)=descriptors[i];
                sampleLabelMat.at<float>(num+PosSamNo+NegSamNo,0)=-1;
            }
        }
        //-------------------------------------------------------------------------------------------
        //              save sampleFeatureVector to file
        //-------------------------------------------------------------------------------------------
        ofstream fout("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/SampleFeatureMat.txt");
        for(int i=0;i!=PosSamNo+NegSamNo;++i)
        {
            fout<<i<<endl;
            for(int j=0;j!=DescriptorDim;++j)
                fout<<sampleFeatureMat.at<float>(i,j)<<" ";
            fout<<endl;
        }
        //-------------------------------------------------------------------------------------------
        //              training SVM
        //-------------------------------------------------------------------------------------------
        CvTermCriteria criteria = cvTermCriteria(CV_TERMCRIT_ITER+CV_TERMCRIT_EPS,1000,FLT_EPSILON);// ending condition:iteration 1000times or error less than FIT_EPSILON
        //parameters of SVM,type:C_SVC,linear kernel function,C=0.01
        CvSVMParams param(CvSVM::C_SVC,CvSVM::LINEAR,0,1,0,0.01,0,0,0,criteria);
        cout<<"start train SVM classifier"<<endl;
        svm.train(sampleFeatureMat,sampleLabelMat,Mat(),Mat(),param);
        cout<<"training ending"<<endl;
        svm.save("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/SVM_HOG.xml");
    }
    else// train is false:reading trained classifier from xml
    {
        svm.load("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/SVM_HOG_2400PosINRIA_12000Neg.xml");
    }
    //-------------------------------------------------------------------------------------------
    //              replace fix classifier in opencv with our trained Svm
    //-------------------------------------------------------------------------------------------
    DescriptorDim=svm.get_var_count();
    int supportVectorNum=svm.get_support_vector_count();
    cout<<"No of support vectors: "<<supportVectorNum<<endl;
    Mat alphaMat=Mat::zeros(1,supportVectorNum,CV_32FC1);
    Mat supportVectorMat=Mat::zeros(supportVectorNum,DescriptorDim,CV_32FC1);
    Mat resultMat=Mat::zeros(1,DescriptorDim,CV_32FC1);
    
    //copy data of support vector to supportVectorMat
    for(int i=0;i!=supportVectorNum;++i)
    {
        const float* pSVData=svm.get_support_vector(i);//return i th data p of sopport vector
        for(int j=0;j!=DescriptorDim;++j)
        {
            supportVectorMat.at<float>(i,j)=pSVData[j];
        }
    }
    //copy data of alpha to alphaMat
    double* pALPHADATA=svm.get_alpha_vector();
    for(int i=0;i!=supportVectorNum;++i)
    {
        alphaMat.at<float>(0,i)=pALPHADATA[i];
    }
    //compute -(alphaMat * supportVectorMat) in resultMat
    resultMat=-1.15*alphaMat*supportVectorMat;
    vector<float>myDetector;
    for(int i=0;i!=DescriptorDim;++i)
    {
        myDetector.push_back(resultMat.at<float>(0,i));
    }
    //add rho
    myDetector.push_back(svm.get_rho());
    cout<<"dimension of detector: "<<myDetector.size()<<endl;
    //set HOG descriptor
    HOGDescriptor myHOG;
    myHOG.setSVMDetector(myDetector);
    //save descriptor
    ofstream fout("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/HOGDetectorForOpenCV.txt");
    for(int i=0;i!=myDetector.size();++i)
    {
        fout<<myDetector[i]<<endl;
    }
    //----------------------------------------------------------------------------------------------
    //                 read image to detect HOG pedestrain
    //----------------------------------------------------------------------------------------------
    Mat src=imread("/Users/new/Desktop/INRIADATA/normalized_images/test/pos/crop_000005.png");
    vector<Rect>found,found_filtered;//rect array
    cout<<"doing multiscale HOG pedestrain detection"<<endl;
    myHOG.detectMultiScale(src,found,0,Size(8,8),Size(48,48),1.15,5);//doing multiscale HOG
    cout<<"find No of rect array: "<<found.size()<<endl;
    //----------------------------------------------------------------------------------------------
    //                 non-maxximal suppression
    //----------------------------------------------------------------------------------------------
    for(int i=0;i!=found.size();++i)
    {
        Rect r=found[i];
        int j=0;
        for(;j!=found.size();++j)
            if(j!=i && (r & found[j])==r)
                break;
        if(j==found.size())
            found_filtered.push_back(r);
    }
    //draw rect window
    for(int i=0;i!=found_filtered.size();++i)
    {
        Rect r=found_filtered[i];
        r.x += cvRound(r.width*0.1);
        r.width=cvRound(r.width*0.8);
        r.y += cvRound(r.height*0.07);
        r.height =cvRound(r.height*0.8);
        rectangle(src,r.tl(),r.br(),Scalar(0,255,0),3);
    }
    imwrite("/Users/new/Desktop/INRIADATA/normalized_images/train/train_thr/ImgProcessed.png",src);
    namedWindow("src",0);
    imshow("src",src);
    waitKey();
    system("pause");
    return 0;
}
